#include "rc_controls.h"
#include "fc.h"
#include "drv_serial.h"
#include "motor_mix.h"
#include <math.h>

#define DBG_TAG "rc.control"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define MAX_SUPPORTED_RC_CHANNEL_COUNT 8
#define MOTOR_IDLING_PWM 200
#define MOTOR_WARMUP_RATE 5
#define RC_LANDING_DEFLECTION 0.25F   // 小于这个时开始下降，大于时取消下降[-1,1]
#define RC_LANDING_DETECT_TIME 1000   // 长按多久开始下降 ms
#define RC_LANDING_POS_DESCEND 0.18F  // 降落时每次下降多少 cm
#define ACC_TOUCHDOWN_THRESHOLD 0.8f  // 加速度碰撞时阈值 g
#define TOF_TOUCHDOWN_THRESHOLD 20    // TOF距离小于这个值时认为到达地面 mm
#define LOWPOWER_LANGING_VOLTAGE 3200 // 电压低于这个值时开始降落 V

ZINO_CONFIG_REGISTER(struct rc_control_max, rc_control_max, 3, 0);

fc_setpoint_t fc_setpoint;
float groundHeight;
float takeoffHeight;

int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
rcControlsConfig_t rc_controler = {
    .max_check = 750,
    .min_check = 250,
    .mid_check = 512,
    .deadband = 10,
    .yaw_deadband = 20,
    .yaw_control_reversed = 1,
    .dt_min = 6,
};

static uint16_t unLockT = 0;
static uint16_t okToArmT = 0;
static uint16_t warnFreq = 180;
static uint8_t beepCnt;
static bool warmUp;
static int16_t warmUpPwm;
const uint16_t ReLockTime = 10000;
static uint8_t rcSticks = 0;
static uint16_t delayMs;
static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f;

void rc_control_max_init(void)
{
    if (rc_control_max()->flag == RC_SAVED_FLAG)
    {
    }
    else
    {
        rc_control_max()->flag = RC_SAVED_FLAG;
        rc_control_max()->level = RC_LEVEL_MAX;
        rc_control_max()->yaw = RC_YAW_MAX;
        rc_control_max()->throttle = RC_THROTTLE_MAX;
    }
}
float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs);
void rcControlsInit(void)
{
}
int fc_crash(void)
{
    return 0;
}
void upateRCcommand(rcControlsConfig_t *rc)
{
    // int16_t center = rc_controler.mid_check - rc->deadband;
    rc->rcCommand[THROTTLE] = rc->rcData[THROTTLE] - rc_controler.mid_check;
    if (rc->rcCommand[THROTTLE] > rc->deadband)
    {
        rc->rcCommand[THROTTLE] -= rc->deadband;
    }
    else if (rc->rcCommand[THROTTLE] < -rc->deadband)
    {
        rc->rcCommand[THROTTLE] += rc->deadband;
    }
    else
    {
        rc->rcCommand[THROTTLE] = 0;
    }
    for (int8_t axis = 0; axis < 3; axis++)
    {
        // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
        int32_t tmp = MIN(ABS(rc->rcData[axis] - rc_controler.mid_check), rc_controler.mid_check);
        if (axis == ROLL || axis == PITCH)
        {
            if (tmp > rc->deadband)
            {
                tmp -= rc->deadband;
            }
            else
            {
                tmp = 0;
            }
            rc->rcCommand[axis] = tmp;
        }
        else
        {
            if (tmp > rc->yaw_deadband)
            {
                tmp -= rc->yaw_deadband;
            }
            else
            {
                tmp = 0;
            }
            rc->rcCommand[axis] = tmp * GET_DIRECTION(rc->yaw_control_reversed);
        }
        if (rc->rcData[axis] < rc->mid_check)
        {
            rc->rcCommand[axis] = -rc->rcCommand[axis];
        }
    }
}
void upateRcDeflection(rcControlsConfig_t *rc)
{
    float center = rc_controler.mid_check - rc->deadband;
    float angleRate;
    rc->RcDeflection[THROTTLE] = (float)rc->rcCommand[THROTTLE] / center;
    for (int8_t axis = 0; axis < 3; axis++)
    {
        // scale rcCommandf to range [-1.0, 1.0]
        float rcCommandf;
        if (axis == FD_YAW)
        {
            rcCommandf = rc->rcCommand[axis] / rcCommandYawDivider;
        }
        else
        {
            rcCommandf = rc->rcCommand[axis] / rcCommandDivider;
        }
        // rc->RcDeflection[axis] = (float)rc->rcCommand[axis] / center;
        rc->RcDeflection[axis] = rcCommandf;
        const float rcCommandfAbs = fabsf(rcCommandf);
        rc->RcDeflectionAbs[axis] = rcCommandfAbs;
        angleRate = applyBetaflightRates(axis, rcCommandf, rcCommandfAbs);
        rc->setpointRate[axis] = constrainf(angleRate, -1998, 1998);
        // rc->setpointRate[axis] = angleRate;
    }
}

void Unlock(rcControlsConfig_t *rc)
{

    if ((rcSticks == ROL_LO + PIT_LO + YAW_HI + THR_LO) && (delayMs > 200)) // 内八解锁
    {
        unLockT += rc->dt_min;
        if (unLockT > 1000)
        {
            FC_ERROR_SET(FC_ARMING_ALERK);

            unLockT = 0;
        }
    }
    else
    {
        unLockT = 0;
    }
    if (FC_ERROR_CHECK(FC_ARMING_ALERK))
    {
        // 电机发出警报声音
        static uint8_t beepT;
        static bool beep;
        beepT += rc->dt_min;
        motor_set_freq(1200); // 1.5kHz电机鸣叫
        if (beepT > warnFreq)
        {
            if (warnFreq > 100)
                warnFreq -= rc->dt_min;
            beepT = 0;
            beep = !beep;
            beepCnt++;
        }

        if (beepCnt > (150 / rc->dt_min))
        {
            FC_ERROR_RESET(FC_ARMING_ALERK);
            if (get_vbat_voltage_mv() < 3300) // NOTE 电量低时不能解锁
            {
                // armingAleakFlag = 0;
                FC_ERROR_SET(FC_LOW_POWER);
                FC_ERROR_RESET(FC_EMERGENCY_STOP);
                FC_ERROR_RESET(FC_OK_TO_ARM);
            }
            else
            {
                FC_ERROR_SET(FC_OK_TO_ARM);
            }

            // FC_ERROR_SET(FC_OK_TO_ARM);
            motor_set_freq(0); // 0时取默认值
        }
        motor_led_state_set(0, RGB_STATE_MODE_FAST);
        motor_led_state_set(1, RGB_STATE_MODE_FAST);
        motor_led_state_set(2, RGB_STATE_MODE_FAST);
        motor_led_state_set(3, RGB_STATE_MODE_FAST);

        currentMixer.throttle = beep ? 20 : 0;
    }
    else
    {
        warnFreq = 180;
        beepCnt = 0;
    }
    if (FC_ERROR_CHECK(FC_OK_TO_ARM))
    {
        okToArmT += rc->dt_min;
        if (!warmUp)
        {

            static uint8_t stepTime;
            if (stepTime++ > 1)
            {
                stepTime = 0;
                if (warmUpPwm < MOTOR_IDLING_PWM)
                {
                    warmUpPwm += MOTOR_WARMUP_RATE;
                }
                else
                {
                    warmUpPwm = MOTOR_IDLING_PWM;
                    warmUp = true;
                }
            }

            currentMixer.throttle = warmUpPwm;
        }
        else
        {

            if ((rc->RcDeflection[THROTTLE] > 0.5f) || (FC_ERROR_CHECK(FC_AUTO_TAKINGOFF)))
            {
                motor_set_freq(0); // 0时取默认值
                // 如果推油门就进入解锁状态
                FC_ERROR_SET(FC_ARMED);
                FC_ERROR_RESET(FC_OK_TO_ARM);
                // FC_ERROR_RESET(FC_EMERGENCY_STOP);
                okToArmT = 0;
                warmUp = false;
                warmUpPwm = 0;
                LOG_D("FC_ARMED\n");
                FC_ERROR_SET(FC_TAKINGOFF);

                motor_led_state_set(0, RGB_STATE_MODE_DOT);
                motor_led_state_set(1, RGB_STATE_MODE_DOT);
                motor_led_state_set(2, RGB_STATE_MODE_DOT);
                motor_led_state_set(3, RGB_STATE_MODE_DOT);
            }
            else
            {
                // NOTE 超时没推油门就上锁
                if ((okToArmT > ReLockTime) || (rc->RcDeflection[THROTTLE] < -0.5f))
                {
                    FC_ERROR_SET(FC_EMERGENCY_STOP);
                    FC_ERROR_RESET(FC_ARMED);
                    FC_ERROR_RESET(FC_OK_TO_ARM);
                }
            }
        }
    }
    else
    {
        warmUp = false;
        warmUpPwm = 0;
        okToArmT = 0;
    }
    if (FC_ERROR_CHECK(FC_EMERGENCY_STOP))
    {
        okToArmT = 0;
        warmUp = false;
        warmUpPwm = 0;
        currentMixer.throttle = 0;
    }
}
void EmergencyStop(rcControlsConfig_t *rc)
{
    // rt_thread_mdelay(10);

    // rt_printf_float("cosTilt: ", getCosTiltAngle(), "\n");
    // NOTE - 急停
    static uint16_t stopT = 0;
    static uint16_t EmStopT = 0;
    if (FC_ERROR_CHECK(FC_ARMED) || FC_ERROR_CHECK(FC_OK_TO_ARM) || FC_ERROR_CHECK(FC_ARMING_ALERK))
    {
        const uint32_t accInpactLimit = 10000;
        bool isAccCrash = ((ABS(fcEstimator.acc[0]) > accInpactLimit) || (ABS(fcEstimator.acc[1]) > accInpactLimit) || (ABS(fcEstimator.acc[2]) > accInpactLimit));
        if (isAccCrash || (getCosTiltAngle() < 0.0f))
        {
            FC_ERROR_RESET(FC_ARMED);
            FC_ERROR_RESET(FC_OK_TO_ARM);
            FC_ERROR_RESET(FC_ARMING_ALERK);
            FC_ERROR_SET(FC_EMERGENCY_STOP);
            LOG_E("Acc_CRASH\n");
        }
    }

    if ((rchandle.rx_data.v.channel[RC_KEY_VALUE] == (1 << FUNPLAY_KEY_LT)) || fc_crash())
    {
        EmStopT += rc->dt_min;
        if (EmStopT > 300)
        {

            FC_ERROR_RESET(FC_ARMED);
            FC_ERROR_RESET(FC_OK_TO_ARM);
            FC_ERROR_RESET(FC_ARMING_ALERK);
            FC_ERROR_SET(FC_EMERGENCY_STOP);
            LOG_D("EMERGENCY_STOP\n");

            EmStopT = 0;
        }
    }
    else
    {
        EmStopT = 0;
    }

    if (FC_ERROR_CHECK(FC_EMERGENCY_STOP))
    {
        FC_ERROR_RESET(FC_ARMED);
        FC_ERROR_RESET(FC_OK_TO_ARM);
        FC_ERROR_RESET(FC_FLIPING_MODE); // FLIPING OVER
        FC_ERROR_RESET(FC_TAKINGOFF);
        FC_ERROR_RESET(FC_AIRBORN);
        FC_ERROR_RESET(FC_LANDING);
        FC_ERROR_RESET(FC_LANDED);
        FC_ERROR_RESET(FC_AUTO_TAKINGOFF);
        okToArmT = 0;
        warmUp = false;
        warmUpPwm = 0;
        //
        currentMixer.throttle = 0;
        stopT += rc->dt_min;

        motor_led_state_set(0, RGB_STATE_MODE_FAST);
        motor_led_state_set(1, RGB_STATE_MODE_FAST);
        motor_led_state_set(2, RGB_STATE_MODE_FAST);
        motor_led_state_set(3, RGB_STATE_MODE_FAST);
        if (stopT > 2000)
        {
            LOG_E("EMERGENCY_STOP\n");
            FC_ERROR_RESET(FC_EMERGENCY_STOP);

            motor_led_state_set(0, RGB_STATE_MODE_BREATH);
            motor_led_state_set(1, RGB_STATE_MODE_BREATH);
            motor_led_state_set(2, RGB_STATE_MODE_BREATH);
            motor_led_state_set(3, RGB_STATE_MODE_BREATH);
            // pro_event_send(PRO_EVENT_STOP);
            stopT = 0;
        }
    }
}
void CalibrationControl(rcControlsConfig_t *rc)
{
    // LOG_D("rlr:%d llr:%d %08X",usercmd->rcChannel[RC_RLR], usercmd->rcChannel[RC_LLR], fc_error_flag);
    // static bool calibrate = false;
    if (FC_ERROR_CHECK(FC_OK_TO_ARM) || FC_ERROR_CHECK(FC_ARMING_ALERK) || FC_ERROR_CHECK(FC_IMU_CALIBRATING) || FC_ERROR_CHECK(FC_ARMED))
        return;
    if ((rcSticks == ROL_HI + PIT_CE + YAW_LO + THR_CE) && (delayMs >= 100)) // 外一校准
    {
        static uint8_t caliCnt = 0;
        if (caliCnt++ > 50)
        {
            caliCnt = 0;
            FC_ERROR_SET(FC_IMU_CALIBRATING);
            AccStartCalibration();
            GyroStartCalibration();
        }
        // LOG_D("CalibrationControl %d %d",caliCnt,FC_ERROR_CHECK(FC_IMU_CALIBRATING));
    }
}
void calculateStickPosition(rcControlsConfig_t *rc)
{

    // 定义一个临时变量stTmp，用于存储当前摇杆位置
    uint8_t stTmp = 0;
    // 遍历四个摇杆
    for (int8_t axis = 0; axis < 4; axis++)
    {
        // 将stTmp右移两位
        stTmp >>= 2;
        // 如果当前摇杆位置小于最小值，则设置stTmp的最低位为1
        if (rc->rcData[axis] < rc->max_check)
        {
            stTmp |= 0x40; // check for MIN
        }
        // 如果当前摇杆位置大于最大值，则设置stTmp的次低位为1
        if (rc->rcData[axis] > rc->min_check)
        {
            stTmp |= 0x80; // check for MAX
        }
    }
    // 如果当前摇杆位置与上一次相同，则增加延迟时间
    if (stTmp == rcSticks)
    {
        if (delayMs < 1000)
        {
            delayMs += rc->dt_min;
        }
    }
    // 否则，将延迟时间置为0
    else
    {
        delayMs = 0;
    }
    // 更新rcSticks为当前摇杆位置
    rcSticks = stTmp;

    // LOG_D("rcSticks: %02X delayMs: %d", rcSticks, delayMs);
}
void rcControls_process(rcControlsConfig_t *rc)
{
    upateRCcommand(rc);
    upateRcDeflection(rc);
    calculateStickPosition(rc);

    Unlock(rc);
    EmergencyStop(rc);
    CalibrationControl(rc);

    fc_landing_handle(rc);
    fc_takingoff_handle(rc);

    static uint8_t lowRcCnt = 0;
    if (FC_ERROR_CHECK(FC_RC_LOST))
    {
        // 持续下降
        if (FC_ERROR_CHECK(FC_AIRBORN))
            FC_ERROR_SET(FC_LANDING);
    }

    // rt_kprintf("")
    if (get_vbat_voltage_mv() < LOWPOWER_LANGING_VOLTAGE)
    {
        FC_ERROR_SET(FC_LOW_POWER);
        lowRcCnt = 100;
    }
    else
    {
        if (lowRcCnt > 0)
        {
            lowRcCnt--;
        }
        else
        {
            FC_ERROR_RESET(FC_LOW_POWER);
        }
    }

    if (FC_ERROR_CHECK(FC_LOW_POWER))
    {
        if (FC_ERROR_CHECK(FC_AIRBORN))
        {
            FC_ERROR_SET(FC_LANDING);
        }
    }

    if (rchandle.rssi > 1)
    {
        if (rchandle.rx_data.v.channel[RC_KEY_VALUE + 1] == ((FUNPLAY_KEY_RT + 1) << 4 | 1))
        {
            /**RT 按了一次 */
            if (FC_STATE_CHECK(FC_AIRBORN))
            {
                FC_STATE_SET(FC_LANDING);
            }
            else
            {
                FC_STATE_SET(FC_ARMED);
                FC_STATE_SET(FC_TAKINGOFF);
                FC_STATE_SET(FC_AUTO_TAKINGOFF);
            }
        }
    }
    // rc_disaming(rc);

    // rt_printf_float("Deflection: ", rc->RcDeflection[0], ", ");
    // rt_printf_float(" ", rc->RcDeflection[1], ", ");
    // rt_printf_float(" ", rc->RcDeflection[2], ", ");
    // rt_printf_float(" ", rc->RcDeflection[3], "\n");
}
void rcControlsUpate(void)
{
    for (int8_t axis = 0; axis < 4; axis++)
    {
        rc_controler.rcData[axis] = rc_controler.mid_check;
    }

    if (rchandle.rssi > 1)
    {
        if ((rchandle.rx_data.v.channel[RC_RLR]) <= 1024)
            rc_controler.rcData[ROLL] = rchandle.rx_data.v.channel[RC_RLR];
        if ((rchandle.rx_data.v.channel[RC_LLR]) <= 1024)
            rc_controler.rcData[YAW] = rchandle.rx_data.v.channel[RC_LLR];
        if ((rchandle.rx_data.v.channel[RC_LLR]) <= 1024)
            rc_controler.rcData[THROTTLE] = rchandle.rx_data.v.channel[RC_LUD];
        if ((rchandle.rx_data.v.channel[RC_RUD]) <= 1024)
            rc_controler.rcData[PITCH] = rchandle.rx_data.v.channel[RC_RUD];
    }

    rcControls_process(&rc_controler);
}
float getRcDeflection(uint8_t axis)
{
    return rc_controler.RcDeflection[axis];
}
float getRcDeflectionAbs(uint8_t axis)
{
    return rc_controler.RcDeflectionAbs[axis];
}
/* Classic Expo */
uint8_t rcExpo[3] = {45, 45, 45};
/* RC Rates */
uint8_t rcRates[3] = {218, 218, 218};
/* Rates Superfactor */
uint8_t rates[3] = {0, 0, 0};
#define RC_RATE_INCREMENTAL 14.54f

/**
 * @brief
 * NOTE rc_betaflightRates.ggb
 * @param axis
 * @param rcCommandf
 * @param rcCommandfAbs
 * @return float -988~988
 */
float applyBetaflightRates(const int axis, float rcCommandf, const float rcCommandfAbs)
{
    if (rcExpo[axis])
    {
        const float expof = rcExpo[axis] / 100.0f;
        rcCommandf = rcCommandf * power3(rcCommandfAbs) * expof + rcCommandf * (1 - expof);
    }
    float rcRate = rcRates[axis] / 100.0f;
    if (rcRate > 2.0f)
    {
        rcRate += RC_RATE_INCREMENTAL * (rcRate - 2.0f);
    }
    float angleRate = 200.0f * rcRate * rcCommandf;
    if (rates[axis])
    {
        const float rcSuperfactor = 1.0f / (constrainf(1.0f - (rcCommandfAbs * (rates[axis] / 100.0f)), 0.01f, 1.00f));
        angleRate *= rcSuperfactor;
    }
    return angleRate;
}
/**
 * @brief Get the Setpoint Rate object
 *
 * @param axis ROLL, PITCH, YAW, THROTTLE
 * @return float -998~998
 */
float getSetpointRate(uint8_t axis)
{
    return rc_controler.setpointRate[axis];
}
/**
 * @brief
 * 当油门持续小于一定时间时，开始慢慢降低期望高度，
 * 如果加速z受到碰撞或者激光检测到碰撞，则降落完成。
 * 如果油门上推，退出降落
 *
 * @param rc
 */
void fc_landing_handle(rcControlsConfig_t *rc)
{
    static uint32_t landingT = 0;
    static uint32_t manualLangdingT = 0;
    static uint32_t landedT = 0;

    // rt_printf_float("thrust: ",fc_control.thrust, " \n");
    if (FC_STATE_CHECK(FC_LANDED))
    {
        if (millis() > (landedT + 3000))
        {
            FC_ERROR_RESET(FC_LANDED);
            motor_led_state_set(0, RGB_STATE_MODE_BREATH);
            motor_led_state_set(1, RGB_STATE_MODE_BREATH);
            motor_led_state_set(2, RGB_STATE_MODE_BREATH);
            motor_led_state_set(3, RGB_STATE_MODE_BREATH);
        }
    }
    if (!FC_STATE_CHECK(FC_AIRBORN))
        return;

    if (rc->RcDeflection[THROTTLE] <= -RC_LANDING_DEFLECTION)
    {
        if (manualLangdingT == 0)
        {
            manualLangdingT = millis();
        }
    }
    else
    {
        manualLangdingT = 0;
    }

    if ((manualLangdingT != 0) && (millis() > (manualLangdingT + RC_LANDING_DETECT_TIME)))
    {

        FC_ERROR_SET(FC_LANDING);
    }

    if (FC_ERROR_CHECK(FC_LANDING))
    {
        if (rc->RcDeflection[THROTTLE] > RC_LANDING_DEFLECTION)
        {
            FC_ERROR_RESET(FC_LANDING);
            manualLangdingT = 0;
        }
        if (landingT == 0)
        {
            landingT = millis();
        }
        if (!FC_ERROR_CHECK(FC_LANDED))
        {
            fc_setpoint.pos.z -= RC_LANDING_POS_DESCEND;
            bool isThrottleDown = (get_throttle() < 0.25f);
            bool isAccTouchDown = (ABS(zdrone_state.acc.z) > ACC_TOUCHDOWN_THRESHOLD);
            bool isLaserTouchdown = (fc_sensor_data.tof.quality > 0) && (fc_sensor_data.tof.range > 0) && (fc_sensor_data.tof.range < TOF_TOUCHDOWN_THRESHOLD);
            // rt_kprintf("tof:%d\t",fc_sensor_data.tof.range);
            // rt_kprintf("accZ:%d\n",(int)(zdrone_state.acc.z * 1000));
            if (FC_ERROR_CHECK(FC_MANUAL_MODE))
            {
                if (isAccTouchDown || isLaserTouchdown || isThrottleDown)
                {
                    FC_ERROR_SET(FC_EMERGENCY_STOP);
                    landingT = 0;
                    manualLangdingT = 0;
                    LOG_D("MANUL_LANDED\n");
                }
            }
            if (isLaserTouchdown)
            {
                LOG_D("Laser Touch down\n");
            }
            if (isAccTouchDown)
            {
                LOG_D("Acc Touch Down\n");
            }
            if (isThrottleDown)
            {
                LOG_D("Throttle Touch Down\n");
            }
            if (isAccTouchDown || isLaserTouchdown || isThrottleDown)
            {
                LOG_D("LANDED\n");
                FC_ERROR_RESET(FC_LANDING);
                FC_ERROR_RESET(FC_ARMED);
                FC_ERROR_RESET(FC_AIRBORN);
                FC_ERROR_SET(FC_LANDED);
                landingT = 0;
                manualLangdingT = 0;
                landedT = millis();
                // pro_event_send(PRO_EVENT_LANDED);
            }
        }
    }
    else
    {
        landingT = 0;
        FC_ERROR_RESET(FC_LANDED);
    }
}
/**
 * @brief
 *
 * @param usercmd
 */
void fc_takingoff_handle(rcControlsConfig_t *rc)
{
    static bool getGroundHeight = false;

    if (FC_ERROR_CHECK(FC_ARMED))
    {
        if (!getGroundHeight)
        {
            getGroundHeight = true;
            groundHeight = zdrone_state.pos.z;
            LOG_D("groundHeight=%d\n", (int)groundHeight);

            // uart4_printf("groundHeight=%d\n", (int)groundHeight);
            // rt_kprintf("groundHeight=%d\n", groundHeight);
        }
    }
    else
    {
        getGroundHeight = false;
        groundHeight = 0;
    }
    // 定义一个静态变量，用于记录起飞时间
    static uint32_t takingoffT = 0;
    takeoffHeight = groundHeight + 120.0f;
    // 检查飞机是否处于起飞状态
    if (FC_ERROR_CHECK(FC_TAKINGOFF))
    {
        // 如果起飞时间未记录，则记录当前时间
        if (takingoffT == 0)
        {
            takingoffT = millis();
            fc_setpoint.vel.z = 0;
            LOG_D("takingoffT=%d\n", takingoffT);
            // uart4_printf("takingoffT=%d\n", takingoffT);
            // rt_kprintf("takingoffT=%d\n", takingoffT);
            // rt_kprintf("takingoffT=%d\n", takingoffT);
        }
        // 检查飞机是否已经起飞
        if (!FC_ERROR_CHECK(FC_AIRBORN))
        {
            // 检查飞机是否处于自动起飞状态
            if (FC_ERROR_CHECK(FC_AUTO_TAKINGOFF))
            {
                // 设置飞机的高度为80.0f
                fc_setpoint.manual[Z] = true;
                if (fc_setpoint.vel.z < 100)
                {
                    fc_setpoint.vel.z += 0.5f;
                }
                // rt_printf_float("setPoint.Pos.z ", takeoffHeight, " \n");
                // uart4_printf("setPoint.Pos.z %d \n", (int)takeoffHeight);
                // uart4_printf("setPoint.vel.z %d \n", (int)fc_setpoint.vel.z);
            }
            // uart4_printf("zdrone_state.pos.z %d \n", (int)zdrone_state.pos.z);
            if (zdrone_state.pos.z >= takeoffHeight/2)
            {
                // uart4_printf("AIRBORNE\n");
                fc_setpoint.manual[Z] = false;
                fc_setpoint.pos.z = takeoffHeight;
                LOG_D("AIRBORNE 1/2\n");
            }
            if(zdrone_state.pos.z >= takeoffHeight)
            {
                fc_setpoint.pos.z = takeoffHeight;
                LOG_D("AIRBORNE\n");
                FC_ERROR_SET(FC_AIRBORN);
            }
            if ((millis() > (takingoffT + 10000)) && (takingoffT != 0))
            {
                fc_setpoint.pos.z = zdrone_state.pos.z;
                FC_ERROR_SET(FC_AIRBORN);
                LOG_D("timeout, AIRBORNE. Height:%d\n", (int)zdrone_state.pos.z);
                // uart4_printf("timeout, AIRBORNE. Height:%d\n", (int)zdrone_state.pos.z);
            }
            if (FC_ERROR_CHECK(FC_AIRBORN))
            {
                fc_setpoint.manual[Z] = 0;
                fc_setpoint.vel.z = 0;
                takingoffT = 0;
                FC_ERROR_RESET(FC_TAKINGOFF);
                FC_ERROR_RESET(FC_AUTO_TAKINGOFF);
                LOG_D("takeofoff done\n");
                // uart4_printf("takeofoff done\n");
            }
        }
    }
    else
    {
        takingoffT = 0;
    }
}
